#ifndef OPENSIM_TOY_PROP_MYO_CONTROLLER_H_
#define OPENSIM_TOY_PROP_MYO_CONTROLLER_H_
/* -------------------------------------------------------------------------- *
 *                      OpenSim: ToyPropMyoController.h                       *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2017 Stanford University and the Authors                *
 * Author(s): Chris Dembia, Shrinidhi K. Lakshmikanth, Ajay Seth,             *
 *            Thomas Uchida                                                   *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied    *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#include "OpenSim/Simulation/Control/Controller.h"
#include "OpenSim/Simulation/Model/Actuator.h"
#include <OpenSim/Common/Constant.h>

namespace OpenSim {
    
/** PropMyoController is a type of Controller that produces a control signal
k*a, where 'k' is the gain property and 'a' is the activation input (note,
however, that the control signal is 0 for a < 0.31). This Controller is
intended to simulate a proportional myoelectric controller [1], and can control
any ScalarActuator (set using the "actuator" socket). This class intended for
demonstration purposes only.
[1] https://en.wikipedia.org/wiki/Proportional_myoelectric_control */
class OSIMEXAMPLECOMPONENTS_API ToyPropMyoController : public Controller {
    OpenSim_DECLARE_CONCRETE_OBJECT(ToyPropMyoController, Controller);

 public:
    // Property of the controller for converting muscle activation into a
    // control signal.
    OpenSim_DECLARE_PROPERTY(gain, double,
                "Gain used to convert muscle activation into a control signal");

    // Socket to the ScalarActuator for which the controller is computing a
    // control signal.
    OpenSim_DECLARE_SOCKET(actuator, OpenSim::ScalarActuator,
         "The actuator for which the controller is computing a control signal");

    // Input the activation signal 'a' to which the controller's output should
    // be proportional.
    OpenSim_DECLARE_INPUT(activation, double, SimTK::Stage::Model,
                 "The signal to which the controller's output is proportional");

    // Output the control signal generated by the controller.
    OpenSim_DECLARE_OUTPUT(myo_control, double, computeControl,
                           SimTK::Stage::Time);

    ToyPropMyoController() {
        constructProperties();
    }

    // Member function for computing the proportional control signal k*a.
    double computeControl(const SimTK::State& s) const
    {
        double activation = getInputValue<double>(s, "activation");

        // TODO add a parameter for this.
        return (activation < 0.31) ? 0. : get_gain() * activation;
    }

    // Member function for adding the control signal computed above into the
    // actuator's Vector of controls.
    void computeControls(const SimTK::State& s,
                         SimTK::Vector& controls) const override
    {
        double signal = computeControl(s);
        const auto& actuator =
            getConnectee<OpenSim::ScalarActuator>("actuator");
        SimTK::Vector thisActuatorsControls(1, signal);
        actuator.addInControls(thisActuatorsControls, controls);
    }

 private:
    void constructProperties() {
        constructProperty_gain(1.0);
    }

}; // end of ToyPropMyoController

} // namespace OpenSim

#endif // OPENSIM_TOY_PROP_MYO_CONTROLLER_H_
